import os
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import ExecuteProcess
from launch_ros.substitutions import FindPackageShare
from launch.actions import IncludeLaunchDescription
from launch.substitutions import LaunchConfiguration
from launch.substitutions import LaunchConfiguration
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    ld = LaunchDescription()
    # use_sim_time = True
    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
    package_name = 'fishbot_description'
    # urdf_name = "robot.urdf"
    urdf_name = "fishbot_base test.urdf"
    pkg_share = FindPackageShare(package=package_name).find(package_name)
    urdf_model_path = os.path.join(pkg_share, f'urdf/{urdf_name}')
    fishbot_navigation2_dir = get_package_share_directory(
        'fishbot_navigation2') 
    map_yaml_path = LaunchConfiguration('map', default=os.path.join(
        fishbot_navigation2_dir, 'maps', 'map06060925.yaml'))
    # 机器人状态发布
    robot_state_publisher_node = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        arguments=[urdf_model_path]
    )

    joint_state_publisher_node = Node(
        package='joint_state_publisher_gui',
        executable='joint_state_publisher_gui',
        name='joint_state_publisher_gui',
        parameters=[{'use_sim_time': use_sim_time}],
        arguments=[urdf_model_path]
    )
    # odom发布
    odom_node = Node(
        package='agv_control',
        executable='odom',
        output='screen'
    )

    # 定义 Gazebo 节点
    gazebo_node = ExecuteProcess(
        cmd=['ros2', 'launch', 'fishbot_description', 'gazebo.launch.py'],
        output='screen'
    )

    # 定义 雷达 节点
    radar_node = ExecuteProcess(
        cmd=['ros2', 'launch', 'sdkeli_ls_udp2',
             'sdkeli_ls1207de_udp_with_1_lidar_launch.py'],
        output='screen'
    )

    # 定义 IMU 节点
    imu_node = ExecuteProcess(
        cmd=['ros2', 'launch', 'fdilink_ahrs', 'ahrs_driver.launch.py'],
        output='screen'
    )

    # 定义 RViz2 节点
    package_name = 'fishbot_navigation2'
    pkg_share = FindPackageShare(package=package_name).find(package_name)
    rviz_config_dir = os.path.join(pkg_share, 'rviz/nav2.rviz')
    print(rviz_config_dir)
    rviz_node = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        # arguments=['-d', rviz_config_dir],
        parameters=[{'use_sim_time': use_sim_time}],
        output='screen')
    
    # 定义 Cartographer 节点
    cartographer_node = ExecuteProcess(
        cmd=['ros2', 'launch', 'fishbot_cartographer', 'cartographer.launch.py'],
        # output='screen'
    )

    map_file = os.path.join(
        get_package_share_directory('fishbot_navigation2'), 'maps', 'map05301649.yaml')
    mapserver_node = Node(
        package='nav2_map_server',
        executable='map_server',
        name='map_server',
        output='screen',
        parameters=[{'use_sim_time': use_sim_time},
                    {'yaml_filename': map_file}]
    )
    lifecycle_node = Node(
        package='nav2_lifecycle_manager',
        executable='lifecycle_manager',
        name='lifecycle_manager_mapper',
        output='screen',
        parameters=[{'use_sim_time': use_sim_time},
                    {'autostart': True},
                    {'node_names': ['map_server']}]
    )

    # 定义 AGV控制 节点
    agv_control = ExecuteProcess(
        cmd=['ros2', 'run', 'agv_control', 'agv'],
        # output='screen'
    )
    # 定义 导航 节点
    nav2_param_path = LaunchConfiguration('params_file', default=os.path.join(
        fishbot_navigation2_dir, 'param', 'fishbot_nav2.yaml'))
    # nav2 = IncludeLaunchDescription(
    #     PythonLaunchDescriptionSource(
    #         [nav2_bringup_dir, '/launch', '/bringup_launch.py']),
    #     launch_arguments={
    #         'map': map_yaml_path,
    #         'use_sim_time': use_sim_time,
    #         'params_file': nav2_param_path}.items(),
    # )
    nav2 = ExecuteProcess(
        cmd=['ros2', 'launch', 'fishbot_navigation2', 'navigation2.launch.py'],
        output='screen'
    )
    # 将所有节点添加到 LaunchDescription
    # ld.add_action(radar_node)
    # ld.add_action(robot_state_publisher_node)
    # # # ld.add_action(joint_state_publisher_node)
    # ld.add_action(radar_node)
    # ld.add_action(imu_node)
    # ld.add_action(odom_node)
    # ld.add_action(agv_control)

    # ld.add_action(gazebo_node)
    # ld.add_action(cartographer_node)
    ld.add_action(nav2)
    ld.add_action(rviz_node)

    # ld.add_action(mapserver_node)
    # ld.add_action(lifecycle_node)

    
    return ld
